hip.ai.pathfinding source code

1 module hip.ai.pathfinding;
2 version(Test):
3 import hip.util.data_structures;
4 
5 private struct AStarNode
6 {
7     AStarNode* previous;
8     int id;
9     int g, h;
10     int f(){return g+h;}
11 }
12 
13 pragma(inline, true)
14 int manhattanHeuristic(int posX, int posY, int targetX, int targetY)
15 {
16     int dx = targetX - posX;
17     int dy = targetY - posY;
18     return (dx < 0 ? -dx : dx) + (dy < 0 ? -dy : dy);
19 }
20 
21 struct AStarResult(T)
22 {
23     bool isPossible;
24     T[] path;
25 }
26 
27 AStarResult!T AStar2D_4Way(T, Q)(ref T[] map, uint startX, uint startY, int columns, uint targetX, uint targetY, Q walls)
28 {
29     import hip.util.array;
30     uint start = startY*columns+startX;
31     uint target = targetY*columns+targetX;
32     AStarResult!T ret;
33 
34     AStarNode[] open = [AStarNode(null, start, 0, manhattanHeuristic(startX, startY, targetX, targetY))];
35     AStarNode[] closed;
36 
37     pragma(inline, true) enum append = (AStarNode* prev, T num)
38     {
39         import std.traits:isArray;
40         static if(is(typeof(walls) == typeof(null))){}
41         else static if(isArray!Q)
42         {
43             for(ulong i = 0; i < walls.length; i++)
44                 if(map[num] == walls[i])
45                     return;
46         }
47         else
48         {
49             if(map[num] == walls)
50                 return;
51         }
52         if(!closed.contains!"id"(num))
53         {
54             int ind = open.indexOf!"id"(num);
55             if(ind != -1)
56             {
57                 if(prev.g+1 < open[ind].g)
58                 {
59                     open[ind].g = prev.g+1;
60                     open[ind].previous = prev;
61                 }
62             }
63             else if(prev != null)
64                 open~=AStarNode(prev, num, prev.g+1, manhattanHeuristic(num%columns, num/columns, targetX, targetY));
65             
66         }
67 
68     };
69     int current;
70     AStarNode* node;
71     while(!open.isEmpty)
72     {
73         ulong lowestF = 0;
74         
75         for(ulong i =0; i < open.length; i++)
76             if(open[i].f < open[lowestF].f)
77                 lowestF = i;
78 
79         closed~=open[lowestF];
80         node = &open[lowestF];
81         current = node.id;
82         
83         if(open[lowestF].id == target)
84         {
85             ret.isPossible = true;
86             break;
87         }
88         int currentColumn = current%columns;
89         //Get the adjacents to the current>
90 
91 
92         //Neighbors part
93 
94         //Up -> Check if there is a row up
95         if((current - columns) >= 0) //Don't access out of bounds
96             append(node, cast(T)(current-columns));
97         //Right -> Check if it does not go a row down
98         if((current + 1)%columns > currentColumn)
99             append(node, cast(T)(current+1));
100         //Down -> Check if there is a row down
101         if(current + columns < map.length) //Don't access out of bounds
102             append(node, cast(T)(current+columns));
103         //Left -> Check if it does not go a row up 
104         if(current-1 >= 0 && (current - 1)%columns < currentColumn)
105         {
106             append(node, cast(T)(current-1));
107         }
108 
109         open.remove(*node);
110         
111     }
112 
113     while(node.previous)
114     {
115         ret.path~= cast(T)node.id;
116         node = node.previous;
117     }
118     ret.path~= cast(T)node.id;
119     return ret;
120 }